import robomaster
import robomaster.camera as camera
import robomaster.flight as flight
import robomaster.robot as robot
import robomaster.config as config
import robomaster.battery as battery
import av
import cv2
import time
import numpy
import threading

lock = threading.Lock()
event = threading.Event()


class MyTello(object):
    EVENT_FLIGHT_DATA = 1
    EVENT_FILE_DATA = 2
    EVENT_INFO_DATA = 3
    EVENT_BATTERY_DATA = 4

    def __init__(self, localIP=''):
        """无人机操作与通信类，能向无人机发送控制命令，能获取无人机拍摄的视频，能获取无人机的飞行状态信息。

        Args:
            localIP (str, optional): 本电脑无线网卡的IP地址，通过ipconfig命令取得，同时，防火墙应为专用网络，否则取不到飞机视频图像. Defaults to ''自动识别.
        """
        super().__init__()
        if localIP != '':
            config.LOCAL_IP_STR = localIP

        self.drone = robot.Drone()
        self.drone.initialize()
        self.flight = flight.Flight(self.drone)
        self.camera = camera.TelloCamera(self.drone)
        self.battery = battery.TelloBattery(self.drone)
        self.Tello_frame = None
        self.video_process = True

        self.video_process_thread = threading.Thread(
            target=self.__Video_process)  # 启动视频流处理线程

        self.date_fmt = '%Y-%m-%d_%H%M%S'  # 日期格式
        time.sleep(2)
        self.video_process_thread.start()

        self.height = 480
        self.width = 600

        time.sleep(2)
        if self.Tello_frame is not None:
            self.height, self.width, depth = self.Tello_frame.shape
            print('resolotion:', self.Tello_frame.shape)

    def get_video_frame(self):
        '''返回当前的无人机视频的一帧图像'''
        return self.Tello_frame

    def __Video_process(self):
        '''单独线程，一直解码并处理来自无人机的视频流'''

        self.camera.start_video_stream(display=False)
        self.camera.set_fps("high")
        self.camera.set_resolution("low")
        self.camera.set_bitrate(6)
        try:
            while self.video_process:
                self.Tello_frame = self.camera.read_cv2_image()
        finally:
            self.camera.stop_video_stream()
        event.set()

    def takeoff(self):
        """无人机起飞"""
        return self.flight.takeoff()

    def land(self):
        """无人机降落"""
        return self.flight.land()

    def counter_clockwise(self, val):
        """无人机逆时针方向旋转

        Args:
            val (整数): 转动的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=0, c=0, d=val * -1)

    def clockwise(self, val):
        """无人机顺时针方向旋转

        Args:
            val (整数): 转动的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=0, c=0, d=val)

    def forward(self, val):
        """无机人向前运动

        Args:
            val (整数): 前进的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=val, c=0, d=0)

    def backward(self, val):
        """无机人向后运动

        Args:
            val (整数): 后退的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=val * -1, c=0, d=0)

    def up(self, val):
        """无机人向上运动

        Args:
            val (整数): 上升的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=0, c=val, d=0)

    def down(self, val):
        """无机人向下运动

        Args:
            val (整数): 下降的速度大小 0-100 的百分比
        """
        self.flight.rc(a=0, b=0, c=val * -1, d=0)

    def left(self, val):
        """无机人向左运动

        Args:
            val (整数): 左移的速度大小 0-100 的百分比
        """
        self.flight.rc(a=val * -1, b=0, c=0, d=0)

    def right(self, val):
        """无机人向右运动

        Args:
            val (整数): 右移的速度大小 0-100 的百分比
        """
        self.flight.rc(a=val, b=0, c=0, d=0)

    def subscribe(self, event, handler):
        if event == self.EVENT_FLIGHT_DATA:
            return self.flight.sub_attitude(freq=5, callback=handler)
        if event == self.EVENT_INFO_DATA:
            return self.drone.sub_drone_info(freq=5, callback=handler)
        if event == self.EVENT_BATTERY_DATA:
            return self.battery.sub_battery_info(freq=5, callback=handler)

    def unsub(self, event):
        if event == self.EVENT_FLIGHT_DATA:
            return self.flight.unsub_attitude()
        if event == self.EVENT_INFO_DATA:
            return self.drone.unsub_drone_info()
        if event == self.EVENT_BATTERY_DATA:
            return self.battery.unsub_battery_info()

    def quit(self):
        """释放无人机操作对象占用的资源"""
        if self.video_process:
            try:
                lock.acquire()
                self.video_process = False
                event.wait()
            except:
                print("quit except.......")
                lock.release()
                event.clear()
        self.drone.close()
        del self.drone


if __name__ == '__main__':
    t = MyTello('192.168.10.2')

    while True:
        f = t.get_video_frame()
        cv2.imshow("t", f)
        if cv2.waitKey(1) & 0xFF == 27:
            break
    cv2.destroyAllWindows()

    t.quit()
